/*
 *  IIR
 */

#include <string.h>
#include "iir.h"

void iirInit(double* x, double* y, int n)
{
    memset(x, 0, sizeof(double)*n);
    memset(y, 0, sizeof(double)*n);
}

double iirRun(double input, double* x, double* y, const double* A, const double* B, int n)
{
    double acc = 0;
    int i;

    acc = input*B[0];
    for(i = 1; i < n; ++i)
    {
        acc += x[i]*B[i];
        acc -= y[i]*A[i];
    }
    for(i = n-1; i > 1; --i)
    {
        y[i] = y[i-1];
        x[i] = x[i-1];
    }
    x[1] = input;
    y[1] = acc;

    return acc;    
}

double iirRunR(double input, double* x, double* y, const double* A, const double* B, int n, double* err)
{
    double acc = 0;
    int i;

    acc = input*B[0];
    for(i = 1; i < n; ++i)
    {
        acc += x[i]*B[i];
        acc -= y[i]*A[i];
    }
    for(i = n-1; i > 1; --i)
    {
        y[i] = y[i-1];
        x[i] = x[i-1];
    }
    x[1] = input;

    float y0 = (float)acc;
    y[1] = y0 + *err;
    *err = acc - y0;

    return y[1];    
}
